"""
任务引导

date: 2025/7/7
author: SiHeng Tang
file: mission_guide.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
import logging
import time
from copy import deepcopy

from async_sche import BaseTaskGen, TaskWrapper, ExitGenerationError
from helpers import mav_messages
from helpers.geo_math_trans import GeoVector
from helpers.mission_mav_events import MavEvents, MavStatus
from helpers.mission_waypoints_gen import plan_path
from mengji import Vehicle
from mj_errors import MJRuntimeError
from mod_loader import ConfigHandler
from task_element import DataEndpoint

logger = logging.getLogger(__name__)
handler = ConfigHandler(__name__)

# 转态转移表 {base_status_code: {trigger_event: target_status_code}}
status_transition_map = {}
# 处理函数映射 {status_code: (handler_cls.on_gen, handler_cls.func, handler.on_exit)}
status_handler_map = {}

DEFAULT_CONFIG = {
    "label": "mg",
    "plane_tele_node": "plane_tele",
    "release_path_node": "release_pf",
    "drop_point_node": "drop_predictor",
    "payload_manage_node": "payload",
    "preselect_targets": [],
    "cruise_default_alt": 35,
    "max_insight_sec": 120.0,
    "insight_timeout_act": "abort",
    "guided_turn_radius": 10,
    "guided_start_max_angle": 30,
    "guided_start_min_distance": 100,
    "drop_distance_tolerance": 5,
    "trans_wp_index": 3,
    "way_back_wp_index": 4,
    "wp_gen_func": "gicg_v2",
    "wp_gen_args": {
        "takeoff_position_lat": 0,
        "takeoff_position_lon": 0,
        "takeoff_direction": 0,
        "takeoff_length": 70,
        "takeoff_alt": 12,
        "cruise_alt": 35,
        "trans_line_distance": 100,
        "target_position_lat": 0,
        "target_position_lon": 0,
        "reenter_line_distance": 200,
        "landing_runway_center_lat": 0,
        "landing_runway_center_lon": 0,
        "landing_runway_direction": 0,
        "way_back_arc_angle": 10,
        "way_back_distance": 400,
        "way_back_alt": 30,
        "runway_entrance_arc_angle": 5,
        "runway_entrance_distance": 200,
        "runway_entrance_alt": 20,
        "approaching_arc_angle": 0,
        "approaching_distance": 100,
        "approaching_alt": 9
    }
}


class BaseStatusHandler:
    """
    基础的状态转换处理类，实际上只是一个静态函数的容器，
    大部分状态状态转移不需要完全重新定义所有处理函数，使用模版加部分重写的方式效率更高。
    """

    @staticmethod
    def on_gen(mg):
        pass

    @staticmethod
    def func(mg):
        pass

    @staticmethod
    def on_exit(mg):
        pass


def add_status_handler(base_status_code: int, trigger_event: int, next_status_code: int):
    """
    修饰器，新增一个状态处理句柄
    :param base_status_code: 基础状态码，即为 from status
    :param trigger_event: 触发事件
    :param next_status_code: 迁移目标状态码，即为 to status
    """

    def func_wrapper(handler_cls):
        # 处理新增状态转移函数，维护状态转移表和状态处理函数映射表
        # 转移表: {state: {event: next_status}}
        # 状态处理函数映射表: {state: (_on_gen, _func, _on_exit)}
        new_handler_col = {trigger_event: next_status_code}
        if base_status_code not in status_transition_map.keys():
            status_transition_map.setdefault(base_status_code, new_handler_col)
        else:
            status_transition_map[base_status_code].update(new_handler_col)

        new_handler_col = {next_status_code: (handler_cls.on_gen, handler_cls.func, handler_cls.on_exit)}
        if next_status_code not in status_handler_map.keys():
            status_handler_map.update(new_handler_col)
        else:
            # 如果对应状态机存在就不在更改，一种转态无论如何进入，处理的函数都应该是相同的
            pass
        return handler_cls

    return func_wrapper


class _StatusLock(object):
    """
    执行状态锁，实现稳定的执行去重等用途。
    """

    # 从最开始的手动设置位到使用静态函数和全局变量，现在又回到了又 MG 主动提供，真是复杂

    def __init__(self):
        self.__lock_dict = {}

    def get_lock(self, lock_name: str):
        if lock_name not in self.__lock_dict:
            self.__lock_dict.setdefault(lock_name, True)
            return True
        else:
            return False

    def exist_lock(self, lock_name: str):
        return lock_name in self.__lock_dict

    def release_lock(self, lock_name: str):
        if lock_name not in self.__lock_dict:
            return False
        else:
            self.__lock_dict.pop(lock_name)
            return True

    def reset_all_lock(self):
        self.__lock_dict.clear()


#####################
# 状态机和 MJ 模块
#####################

class MissionStatusMachine:
    """
    任务有限状态机（MFSM）最大特点是存储了三个控制器函数，为了适应生成器的模版要求，
    三个控制器函数大致对应了生成器三阶段，但是有一些不同的地方::

        1. 执行被包装在 MissionGuidance 生成器中，函数由于不是成员函数需要将首个参数设置为生成器对象
        2. _on_exit 执行的时机不仅仅是生成器生命周期终止，而是在状态切换时也被执行
        3. 这种注入式执行的方式没有显化的初始化和销毁流程，控制器在使用某些对象时需要严格检查对象是否存在
    """

    def __init__(self, mission_guidance):
        self._status = MavStatus.MJ_SYSTEM_INITIAL

        mission_guidance.controller_on_gen = None
        mission_guidance.controller_func = None
        mission_guidance.controller_on_exit = None

        # 给一个上电事件
        # self.send_event(mission_guidance, MavEvents.MAV_POWER_ON)

    def set_status_to(self, mission_guidance, status_code: int) -> bool:
        """
        直接设置状态机状态
        :param mission_guidance: 任务引导实例
        :param status_code: 状态码
        :return: 转换成功返回 True，失败返回 False
        """
        # 先查询目标状态是否存在
        # 目标状态存在则执行标准转态转换流程，不存在则拒绝转换
        if status_code in status_handler_map.keys():
            if status_code == self._status:
                logger.warning(f"[MFSM] closed loop status trans {status_code}: {MavStatus.string_dict[status_code]}")

            ctl_on_gen, ctl_func, ctl_on_exit = status_handler_map[status_code]

            if mission_guidance.controller_on_exit:
                mission_guidance.controller_on_exit(mission_guidance)
                mission_guidance.sl.reset_all_lock()
            mission_guidance.controller_on_gen = ctl_on_gen
            mission_guidance.controller_func = ctl_func
            mission_guidance.controller_on_exit = ctl_on_exit

            old_status, self._status = self._status, status_code

            logger.info(f"[MFSM] set status machine to status {status_code}: {MavStatus.string_dict[status_code]}")
            return True
        else:
            logger.info(f"[MFSM] failed setting status from {self._status}: {MavStatus.string_dict[self._status]} to "
                        f"target satus code: {status_code} not available in handler map!")
            return False

    def send_event(self, mg, event: int):
        """
        接收一个事件，尝试产生一个状态转移。
        :param mg: 任务管理器对象
        :param event: 事件代码
        :return: 转换成功返回 True 失败返回 False
        """
        if self._status in status_transition_map:
            available_transition = status_transition_map[self._status]
        else:
            return

        if event not in available_transition.keys():
            logger.debug(f"[MFSM] event '{MavEvents.string_dict[event]}' "
                         f"ignored in state '{MavStatus.string_dict[self._status]}'")
        else:
            target_status = available_transition[event]
            self.set_status_to(mg, target_status)

    @property
    def status(self):
        return self._status


class MissionGuidance(BaseTaskGen):
    """
    任务引导模块管理整个任务流程，核心使用状态机实现任务转态切换和响应，
    通过解析飞行器遥测，从遥测数据生成事件。
    任务引导也直接提供部分接口，实现任务特定的事件。
    """

    def __init__(self, veh: Vehicle, config: dict):
        super().__init__(priority=TaskWrapper.IDLE_PRIORITY + 5)
        self.veh = veh
        self.cfg = config
        self.recv_gps_ep = DataEndpoint()
        self.recv_fm_ep = DataEndpoint()
        self.recv_mission_current_ep = DataEndpoint()
        self.recv_drop_point_ep = DataEndpoint()
        self.recv_hdg_advice_ep = DataEndpoint()

        self.controller_on_gen = None
        self.controller_func = None
        self.controller_on_exit = None
        self.jump_controller_func = False

        # 解析配置文件中的预定义目标坐标
        self.targets_position: list[GeoVector] = []
        for pos_tup in self.cfg["preselect_targets"]:
            self.targets_position.append(GeoVector(lat=pos_tup[0], lon=pos_tup[1]))
        self.target_position: GeoVector | None = None

        self.sm = MissionStatusMachine(self)
        self.sl = _StatusLock()
        self.last_events_gen_time = time.time()
        self.events = []

        self.generate_manual_interp = False
        self.new_target_position = False

        self.start_insight_time = 0

    def _on_gen(self):
        # 只跳过控制器的 func 函数体，但是依旧处理状态机的状态转换
        try:
            if self.controller_on_gen:
                self.controller_on_gen(self)
        except ExitGenerationError:
            self.jump_controller_func = True

    async def _func(self):
        if self.controller_func and not self.jump_controller_func:
            self.controller_func(self)
        self.jump_controller_func = False

        if self.sm.status == MavStatus.INSIGHT and self.sl.get_lock("insight_status"):
            self.start_insight_time = time.time()

        # 限制最大事件发送速率
        if (time.time() - self.last_events_gen_time) > 0.2:
            self.__analyze_plane_events()
            self.last_events_gen_time = time.time()

        if self.events:
            self.sm.send_event(self, self.events.pop(0))

    def _on_exit(self):
        if self.controller_on_exit:
            self.controller_on_exit(self)

        self.recv_gps_ep = None
        self.recv_fm_ep = None
        self.recv_mission_current_ep = None
        self.recv_drop_point_ep = None

        self.controller_on_gen = None
        self.controller_func = None
        self.controller_on_exit = None

        self.veh = None
        self.events.clear()
        self.sm = None

    def __analyze_plane_events(self):
        """
        进行事件条件分析，将新产生的事件添加到事件列表
        """
        pt = self.veh.get_a(self.cfg["plane_tele_node"])
        gps_payload = self.recv_gps_ep.get_dp().get_a("payload")
        fm_payload = self.recv_fm_ep.get_dp().get_a("payload")
        mc_payload = self.recv_mission_current_ep.get_dp().get_a("payload")

        # 所有遥测都生效的情况下才解析事件
        if not (gps_payload and fm_payload and mc_payload and pt):
            return

        now_pos_gv = GeoVector(lat=gps_payload["lat"], lon=gps_payload["lon"])

        # 事件采用列表形式返回，避免出现一个事件重复发生阻塞其他事件，事件重复发生不重要，状态机会正确响应每一个事件
        if fm_payload["MAV_MODE_FLAG_SAFETY_ARMED"]:
            self.events.append(MavEvents.MAV_ARMED_THROTTLE)

        if not fm_payload["MAV_MODE_FLAG_SAFETY_ARMED"]:
            self.events.append(MavEvents.MAV_DISARM_THROTTLE)

        if fm_payload["custom_mode"] != mav_messages.AP_PLANE_MODE_INITIALIZING and pt.get_home_position():
            self.events.append(MavEvents.MAV_GPS_HOMED)

        if gps_payload["relative_alt"] > 10:
            self.events.append(MavEvents.MAV_FINISH_TAKEOFF)

        if self.cfg["trans_wp_index"] == mc_payload["seq"]:
            self.events.append(MavEvents.MAV_REACH_WP_TRANS)

        if self.sm.status == MavStatus.POSITIONING:
            is_hdg = abs(GeoVector.get_hdg_from_gv(now_pos_gv.diff(self.target_position)) - gps_payload["hdg"]) < \
                     self.cfg["guided_start_max_angle"]
            is_dist = now_pos_gv.distance(GeoVector(lat=gps_payload["lat"], lon=gps_payload["lon"])) < self.cfg[
                "guided_start_min_distance"]

            if is_dist and is_hdg:
                self.events.append(MavEvents.MAV_POSITION_DOWN)

        if self.new_target_position:
            self.events.append(MavEvents.MAV_GET_TARGET_POSITION)
            self.new_target_position = False

        try:
            if self.veh.get_a(self.cfg["payload_manage_node"]).has_activated():
                self.events.append(MavEvents.MAV_RELEASE_PAYLOAD)
        except MJRuntimeError:
            pass

        if self.generate_manual_interp:
            self.events.append(MavEvents.MAV_MANUAL_INTERP)
            self.generate_manual_interp = False

        if bool(self.start_insight_time) and ((time.time() - self.start_insight_time) > self.cfg["max_insight_sec"]):
            self.events.append(MavEvents.MAV_WAIT_TIMEOUT)
            self.start_insight_time = 0

        self.events.append(MavEvents.MAV_POWER_ON)

    #########################
    # 任务管理接口
    ########################3
    def manual_interrupt(self):
        """
        手动中断接口，没有在 api 服务器上实现
        :return:
        """
        self.generate_manual_interp = True

    def activate_bomb(self):
        """
        激活释放机构，此方法供状态响应函数使用。
        """
        self.veh.get_a(self.cfg["payload_manage_node"]).activate_release_sequence()

    def set_target_position(self, pos: GeoVector | int):
        """
        设置目标坐标点，此方法供状态响应函数使用。
        :param pos: GeoVector 目标坐标点，或者目标整数索引
        """
        if type(pos) == int:
            if pos > len(self.targets_position):
                return
            self.target_position = self.targets_position[pos]

        elif type(pos) == GeoVector:
            self.target_position = pos

        self.veh.get_a(self.cfg["release_path_node"]).set_target_pos(self.target_position.lat,
                                                                     self.target_position.lon)
        self.new_target_position = True

    def add_new_target(self, new_pos: GeoVector):
        if not isinstance(new_pos, GeoVector):
            return -1

        self.targets_position.append(new_pos)
        return self.targets_position.index(new_pos)


class ApiCommandHandler:
    def __init__(self, mg: MissionGuidance):
        self.mg: MissionGuidance = mg

    def command_ADD_TARGET_POSITION(self, rpc_params: list):
        """
        添加一个新的可选目标
        :param rpc_params: 列表，只允许包含一个目标
        :return: 添加成功返回 0
        """
        if not isinstance(rpc_params, list):
            return -1

        if len(rpc_params) != 2:
            return -1

        self.mg.add_new_target(GeoVector(lat=rpc_params[0], lon=rpc_params[1]))

        return 0

    def topic_LIST_TARGETS(self, rpc_params: str):
        """
        订阅整个目标列表，每次订阅刷新都会返回
        :param rpc_params: RPC 话题参数
        :return: 目标列表
        """
        if rpc_params == "subscribe":
            return 0

        if rpc_params == "unsubscribe":
            return 0

        if rpc_params == "mailbox_update":
            return [[i, t.lat, t.lon] for i, t in enumerate(self.mg.targets_position)]

        return -1

    def command_SELECT_TARGET(self, rpc_params: int):
        """
        选择一个目标
        :param rpc_params: 目标索引值
        :return: 选择成功返回 0
        """
        if not isinstance(rpc_params, int):
            return -1

        if rpc_params > len(self.mg.targets_position):
            return -2

        self.mg.set_target_position(rpc_params)

        return 0

    def command_RETRY_POSITIONING(self, rpc_params):
        """
        重新调整打击位置对准目标
        :param rpc_params: 只要此函数被调用即为有效，此函数不使用此参数
        :return: 默认返回 0
        """
        self.mg.sm.set_status_to(self.mg, MavStatus.POSITIONING)
        return 0

    def command_MANUAL_RELEASE(self, rpc_params: list):
        """
        手动强制释放载荷，与直接通过载荷管理接口交互不同，此方法触发状态机事件
        :param rpc_params: 调用此函数时，参数必须是一个空列表
        :return: 执行成功返回 0
        """
        if not isinstance(rpc_params, list):
            return -1

        self.mg.activate_bomb()

        return 0


#####################
# 状态机响应函数
#####################
@add_status_handler(MavStatus.MJ_SYSTEM_INITIAL, MavEvents.MAV_POWER_ON, MavStatus.PLANE_INITIALIZE)
class PlaneInitializeStatusHandler(BaseStatusHandler):

    @staticmethod
    def on_gen(mg: MissionGuidance):
        if mg.sl.get_lock("plane_initialize"):
            logger.info("mission controller initializing")


@add_status_handler(MavStatus.PLANE_INITIALIZE, MavEvents.MAV_GPS_HOMED, MavStatus.READY)
@add_status_handler(MavStatus.BACK_HOME, MavEvents.MAV_DISARM_THROTTLE, MavStatus.READY)
class ReadyStatusHandler(BaseStatusHandler):

    @staticmethod
    def on_gen(mg: MissionGuidance):
        """
        飞行器就绪，是一个通用的空闲等待状态，
        对于初次任务执行，获取 HOME 位置，计算模板航线，上传航线。
        任务结束后什么都不会做。
        """
        # 等待飞行器 HOME 位置收敛
        ptele_node = mg.veh.get_a(mg.cfg["plane_tele_node"])
        home = ptele_node.get_home_position()

        if home is None:
            return

        # 调用航线规划器计算模板航线
        planner_args = deepcopy(mg.cfg["wp_gen_args"])
        planner_args.update({
            "takeoff_position_lat": home["latitude"],
            "takeoff_position_lon": home["longitude"]
        })
        wp_list = plan_path(planner_args, mg.cfg["wp_gen_func"])

        # 如果之前没有上传过航线，现在就上传航线并切换到 AUTO 模式
        # 模式切换不是很重要，不会引起重传，如果失败就飞行员手动切换
        if mg.sl.get_lock("upload_wp_list"):
            if ptele_node.send_wp_list(wp_list):
                logger.warning("plane has uploaded way point implement")

                if ptele_node.send_flight_mode(mav_messages.AP_PLANE_MODE_AUTO, check=True):
                    logger.warning("plane has been switched to auto mode and ready to arm")
            else:
                mg.sl.release_lock("upload_wp_list")
        else:
            # 这个模式还有可能是常规降落之后进入
            pass


@add_status_handler(MavStatus.READY, MavEvents.MAV_ARMED_THROTTLE, MavStatus.TAKEOFF)
class TakeoffStatusHandler(BaseStatusHandler):

    @staticmethod
    def on_gen(mg: MissionGuidance):
        if mg.sl.get_lock("enter_take_off"):
            logger.warning(f"ready to take off from home, throw the plane into air")

    @staticmethod
    def on_exit(mg: MissionGuidance):
        logger.warning(f"finishing taking off")


@add_status_handler(MavStatus.TAKEOFF, MavEvents.MAV_FINISH_TAKEOFF, MavStatus.WORK_WAY)
class WorkWayStatusHandler(BaseStatusHandler):

    @staticmethod
    def on_gen(mg: MissionGuidance):
        if mg.sl.get_lock("enter_work_way"):
            logger.warning(f"on the way to mission area, using manual interp to stop the execute")

    @staticmethod
    def on_exit(mg: MissionGuidance):
        logger.warning(f"arrival mission area")


@add_status_handler(MavStatus.WORK_WAY, MavEvents.MAV_REACH_WP_TRANS, MavStatus.INSIGHT)
class InsightStatusHandler(BaseStatusHandler):

    @staticmethod
    def on_gen(mg: MissionGuidance):
        if mg.sl.get_lock("enter_insight_status"):
            logger.warning(f"searching for target")

            # 如果预选了目标，将预选目标作为侦查对象，如果没有，原地盘旋等待
            if bool(mg.targets_position):
                insight_point = mg.targets_position[len(mg.targets_position) // 2]
                mg.veh.get_a(mg.cfg["plane_tele_node"]).send_flight_mode(mav_messages.AP_PLANE_MODE_GUIDED)
                mg.veh.get_a(mg.cfg["plane_tele_node"]).send_guided_waypoint(lat=insight_point.lat,
                                                                             lon=insight_point.lon,
                                                                             alt=mg.cfg["cruise_default_alt"])
            else:
                mg.veh.get_a(mg.cfg["plane_tele_node"]).send_flight_mode(mav_messages.AP_PLANE_MODE_LOITER)


@add_status_handler(MavStatus.INSIGHT, MavEvents.MAV_GET_TARGET_POSITION, MavStatus.POSITIONING)
@add_status_handler(MavStatus.WAIT_TIMEOUT, MavEvents.MAV_GET_TARGET_POSITION, MavStatus.POSITIONING)
class PositioningStatusHandler(BaseStatusHandler):

    @staticmethod
    def on_gen(mg: MissionGuidance):
        if mg.sl.get_lock("enter_positioning_status"):
            logger.warning("positioning carry aircraft for attack line")
            mg.veh.get_a(mg.cfg["plane_tele_node"]).send_flight_mode(mav_messages.AP_PLANE_MODE_GUIDED)

    @staticmethod
    def func(mg: MissionGuidance):
        # 此状态判断是否距离目标过近，小于设定的最小引导距离将引发飞行器位置调整 (positioning)
        # 位置调整简单的将飞行器位置和目标位置连线，并延长到最小引导距离，在此处划定 GUIDED 航点
        now_pos_dict = mg.recv_gps_ep.get_dp().get_a("payload")

        if not now_pos_dict:
            return

        ac_now_pos_gv = GeoVector(lat=now_pos_dict["lat"], lon=now_pos_dict["lon"])

        if mg.sl.exist_lock("sent_reposition_request"):
            return

        if ac_now_pos_gv.distance(mg.target_position) < mg.cfg["guided_start_min_distance"]:
            go_away_gv = ac_now_pos_gv.add_difference(
                mg.target_position.diff(ac_now_pos_gv).get_unit_vector() * (
                        mg.cfg["guided_start_min_distance"] + mg.cfg["guided_turn_radius"]))
            mg.veh.get_a(mg.cfg["plane_tele_node"]).send_guided_waypoint(lat=go_away_gv.lat,
                                                                         lon=go_away_gv.lon,
                                                                         alt=mg.cfg["cruise_default_alt"])
            mg.sl.get_lock("sent_reposition_request")


@add_status_handler(MavStatus.INSIGHT, MavEvents.MAV_WAIT_TIMEOUT, MavStatus.WAIT_TIMEOUT)
class WaitingTimeout(BaseStatusHandler):

    @staticmethod
    def on_gen(mg: MissionGuidance):
        if mg.cfg["insight_timeout_act"] == "abort":
            mg.activate_bomb()

        elif mg.cfg["insight_timeout_act"] == "attack":
            insight_point = mg.targets_position[len(mg.targets_position) // 2]
            mg.set_target_position(insight_point)


@add_status_handler(MavStatus.POSITIONING, MavEvents.MAV_POSITION_DOWN, MavStatus.ATTACK)
class AttackStatusHandler(BaseStatusHandler):

    @staticmethod
    def on_gen(mg):
        if mg.sl.get_lock("enter_attack_status"):
            logger.warning(f"enter attack line")
            mg.veh.get_a(mg.cfg["plane_tele_node"]).send_flight_mode(mav_messages.AP_PLANE_MODE_GUIDED)
            mg.veh.get_a(mg.cfg["drop_point_node"]).set_enable_drop_predict()
            mg.veh.get_a(mg.cfg["release_path_node"]).set_enable_path_advice()

        else:
            if mg.recv_hdg_advice_ep.detect_new_in_dp():
                mg.sl.get_lock("drop_point_updated")

    @staticmethod
    def func(mg: MissionGuidance):
        advice_hdg_dict = mg.recv_hdg_advice_ep.get_dp().get_a('payload')
        now_pos_dict = mg.recv_gps_ep.get_dp().get_a("payload")
        now_drop_point = mg.recv_drop_point_ep.get_dp().get_a('payload')

        if not (now_pos_dict and advice_hdg_dict):
            return

        ac_now_pos_gv = GeoVector(lat=now_pos_dict["lat"], lon=now_pos_dict["lon"])

        # 这里的距离判断频率为系统允许的最大频率，但是 MJ 本身并不是为精确计时空投开发的，所以也不会有太好的效果
        if mg.target_position.distance(now_drop_point) < mg.cfg["drop_distance_tolerance"]:
            mg.activate_bomb()

        # 刷新飞行航向只在新航向更新时进行
        if mg.sl.release_lock("drop_point_updated"):
            # 实现航向引导，在飞行器目前位置为起点，航向方向一公里处画一个航点
            tgt_gv = ac_now_pos_gv.add_difference(
                GeoVector.get_gv_from_hdg(advice_hdg_dict["advice_hdg"]) * 1000)
            mg.veh.get_a(mg.cfg["plane_tele_node"]).send_guided_waypoint(lat=tgt_gv.lat,
                                                                         lon=tgt_gv.lon,
                                                                         alt=mg.cfg["cruise_default_alt"])

            logger.info(f"Updated heading bug to {advice_hdg_dict['advice_hdg']}")

    @staticmethod
    def on_exit(mg):
        logger.warning(f"finished attack mission")

        mg.veh.get_a(mg.cfg["drop_point_node"]).set_enable_drop_predict(False)
        mg.veh.get_a(mg.cfg["release_path_node"]).set_enable_path_advice(False)


@add_status_handler(MavStatus.ATTACK, MavEvents.MAV_RELEASE_PAYLOAD, MavStatus.BACK_HOME)
@add_status_handler(MavStatus.WAIT_TIMEOUT, MavEvents.MAV_RELEASE_PAYLOAD, MavStatus.BACK_HOME)
class BackHomeStatusHandler(BaseStatusHandler):

    @staticmethod
    def on_gen(mg: MissionGuidance):
        if mg.sl.get_lock("enter_back_home"):
            mg.veh.get_a(mg.cfg["plane_tele_node"]).send_mission_current(mg.cfg["way_back_wp_index"])
            mg.veh.get_a(mg.cfg["plane_tele_node"]).send_flight_mode(mav_messages.AP_PLANE_MODE_AUTO)
            logger.warning(f"starting backing home sequence")

    @staticmethod
    def on_exit(mg):
        logger.warning(f"mission finished")


@add_status_handler(MavStatus.PLANE_INITIALIZE, MavEvents.MAV_ARMED_THROTTLE, MavStatus.RESCUE)
@add_status_handler(MavStatus.READY, MavEvents.MAV_MANUAL_INTERP, MavStatus.RESCUE)
@add_status_handler(MavStatus.TAKEOFF, MavEvents.MAV_MANUAL_INTERP, MavStatus.RESCUE)
@add_status_handler(MavStatus.WORK_WAY, MavEvents.MAV_MANUAL_INTERP, MavStatus.RESCUE)
@add_status_handler(MavStatus.INSIGHT, MavEvents.MAV_MANUAL_INTERP, MavStatus.RESCUE)
@add_status_handler(MavStatus.ATTACK, MavEvents.MAV_MANUAL_INTERP, MavStatus.RESCUE)
@add_status_handler(MavStatus.BACK_HOME, MavEvents.MAV_MANUAL_INTERP, MavStatus.RESCUE)
class RescueStatusHandler(BaseStatusHandler):

    @staticmethod
    def on_gen(mg: MissionGuidance):
        if mg.sl.get_lock("rescue_routine"):
            mg.veh.get_a(mg.cfg["plane_tele_node"]).send_flight_mode(mav_messages.AP_PLANE_MODE_RTL)
            logger.warning(f"RESCUE MODE, DEFAULT RETURN TO HOME!")


#####################
# 配置文件回调函数
#####################

# noinspection SpellCheckingInspection
@handler.add_handler("unit_proc:mg")
def load_mg_config(veh: Vehicle, cfg_section: dict):
    logger.debug(f"mission guide load config {cfg_section=}")

    cfg = deepcopy(DEFAULT_CONFIG)
    cfg.update(cfg_section)

    mg = MissionGuidance(veh, cfg)
    api_handler = ApiCommandHandler(mg)

    veh.reg_parts(is_sche=True, **{cfg_section["label"]: mg})
    veh.get_a("api").add_command("mg/addtgt", api_handler.command_ADD_TARGET_POSITION)
    veh.get_a("api").add_command("mg/slctgt", api_handler.command_SELECT_TARGET)
    veh.get_a("api").add_command("mg/rtypos", api_handler.command_RETRY_POSITIONING)
    veh.get_a("api").add_command("mg/rlspld", api_handler.command_MANUAL_RELEASE)
    veh.get_a("api").add_topic("mg/lsttgt", api_handler.topic_LIST_TARGETS)
